/******************************************************************************
**
** File name:	rtos_debug.S
**
** Description:	RTOS v2. Implementation of the RTOS debug services.
**				The RTOS functions required to debug either application
**				and/or RTOS itself.
**
** RTOS ver:	2.0
**
** Modules/Functions:
**
**		void			RTOS_HaltOS			( void )
**		RTOS_PID_t *	RTOS_GetTopTaskID	( void )
**		RTOS_PID_t *	RTOS_GetRunTaskID	( void )
**		RTOS_PID_t *	RTOS_GetSysLevel	( void )
**
** <<NEW>> RTOS Serial Debug Protocol v2 (optimized):
**
**		void			RTOS_DebugOutSync   ( void * )
**		void			RTOS_DebugOutAddr   ( void * )
**		void			RTOS_DebugOutData	( void * , RTOS_U8_t )
**		void			RTOS_DebugOutDump	( void )
**
**  1. Sending pointer to the constant string in ROM:
**
**		bits		7654 3210	comments
**		=======================================================================
**		byte_0		0110 cscs	bits 7..4 = ID; bits 3..0 = CS ((byte_1 XOR byte_2) & 0x0F)
**		byte_1		dddd dddd	address low byte  
**		byte_2		dddd dddd	address high byte 
**
**	2. Sending data buff
**
**		bits		7654 3210	comments
**		=======================================================================
**		byte_0		1001 nnnn	bits 7..4 = ID; bit 3..0 - (data_size-1)
**		byte_1		dddd dddd	data byte 1
**		...			...	
**		byte_N		dddd dddd	data byte N (N = data_size)
**		byte_N+1	cscs cscs	check sum ( 0 XOR byte_1 XOR byte_2 ... XOR byte_N )
**								particular case: for 1 byte (N = 1) check sum == byte
**								1001 0000  dddd dddd dddd dddd  
**
** <<OBSOLETE>> RTOS Serial Debug protocol description:
**
**		void			RTOS_DebugOutPtr	( void * )
**		void			RTOS_DebugOutByte   ( RTOS_U8_t )
**		void			RTOS_DebugOutBuff	( void * , RTOS_U8_t )
**
**	1. Constant string message	RTOS_DebugOutPtr ( addr )
**
**		ADDR_LO		xxxx xxxx	ADDR_LO (bit 0..7) address low byte
**		ID,ADDR_HI	0xxx xxxx	ID = 0 (bit 7) const string message ID, ADDR_HI (bit 0..6) address high byte
**		CRC			xxxx xxxx	CRC (bit 0..7) = <ADDR_LO> XOR <ADDR_HI>
**
**	2. Single byte				RTOS_DebugOutByte ( data )
**
**		DATA		xxxx xxxx	8 bits of the data byte
**		ID,NUM		1000 0000	ID = 1 (bit 7) data message ID, NUM = 0 (bit 0..6) byte order
**		CRC			xxxx xxxx	CRC (bit 0..7) = <DATA> XOR <ID,NUM>
**
**	3. Data buffer				RTOS_DebugOutBuff ( &data , num )
**
**		DATA		xxxx xxxx	8 bits of the data byte
**		ID,NUM		1xxx xxxx	ID = 1 (bit 7) data message ID, NUM = 0 (bit 0..6) bytes to go + 1
**		CRC			xxxx xxxx	CRC (bit 0..7) = <DATA> XOR <ID,NUM>
**
**		void			RTOS_DebugOutAddr	( void * )
**		void			RTOS_DebugOutData	( void * , RTOS_U8_t )
**
** Notes:	This module features are for debug purpose only! Not for use
**			in release versions.
**
*******************************************************************************
**
** History:
**
**	10/10/2011 - AK - Moving to RTOS version 2. Added services to access
**					RtosKernelData structure for debug purposes.
**	07/26/2012 - AK - RTOS API is downgraded to comply with earliest rtos v2.
**  08/08/2012 - AK - RTOS source code is checked for byte-by-byte complience 
**					with MPP binary (Dec 8, 2011).
**	08/13/2012 - AK - RTOS_DebugOutXXX() services prototype.
**	08/24/2012 - AK - Optimized RTOS debug protocol
** 		
******************************************************************************/
#include        "rtos.inc"

        _MODULE(rtos_debug)

    _EXTERN(RtosKernelData)

        _CODESECTION

/******************************************************************************
** Prototype:	void RTOS_HaltOS(void)
** Description:	Lock the system. 
** Passed:		none
** Returns:		never returns
** Uses:		nothing
** Calls:		none
** Assumes:		Debug mode
** Notes:		For Debug purpose only! Don't use in release version!
******************************************************************************/
    _FUNCTION(RTOS_HaltOS)

RTOS_HaltOS:

	break

    cli						;; BeginCritical			

rt_ht00:

    rjmp    rt_ht00			;; infinite loop

    _ENDFUNC	;; _FUNCTION(RTOS_Halt)

/******************************************************************************
** Prototype:	pRtosTaskID RTOS_GetTopTaskID(void)
** Description:	
** Passed:		none
** Returns:		never returns
** Uses:		nothing
** Calls:		none
** Assumes:		Debug mode
** Notes:		For Debug purpose only! Don't use in release version!
******************************************************************************/
	_FUNCTION(RTOS_GetTopTaskID)

RTOS_GetTopTaskID:

    lds     r1h, RtosKernelData+RunQueue+NextH
    lds     r1l, RtosKernelData+RunQueue+NextH

	ret

	_ENDFUNC	;; _FUNCTION(RTOS_GetTopTaskID)

/******************************************************************************
** Prototype:	pRtosTaskID RTOS_GetRunTaskID(void)
** Description:	
** Passed:		none
** Returns:		never returns
** Uses:		nothing
** Calls:		none
** Assumes:		Debug mode
** Notes:		For Debug purpose only! Don't use in release version!
******************************************************************************/
	_FUNCTION(RTOS_GetRunTaskID)

RTOS_GetRunTaskID:

    lds     r1h, RtosKernelData+Running+NextH
    lds     r1h, RtosKernelData+Running+NextL

	ret

	_ENDFUNC	;; _FUNCTION(RTOS_GetRunTaskID)

/******************************************************************************
** Prototype:	unsigned char RTOS_GetSysLevel(void)
** Description:	
** Passed:		none
** Returns:		never returns
** Uses:		nothing
** Calls:		none
** Assumes:		Debug mode
** Notes:		For Debug purpose only! Don't use in release version!
******************************************************************************/
	_FUNCTION(RTOS_GetSysLevel)
	
RTOS_GetSysLevel:

    lds     r1l, RtosKernelData+SysLevel

	ret
	
	_ENDFUNC	;; _FUNCTION(RTOS_GetSysLevel)


/******************************************************************************
** Prototype:	unsigned char RTOS_DebugOutPtr( void * data )
** Description:	This function put the passed parameter into the debug USART
**				queue. The data will be actually sent from RTOS idle task.
** Passed:		p1l:p1h - pointer to the constant debug string.
** Returns:		none
** Uses:		none
** Calls:		none
** Assumes:		Debug sting is the constant located in program memory.
** Notes:		For Debug purpose only! Don't use in release version!
******************************************************************************/
	_FUNCTION(RTOS_DebugOutPtr)
	
RTOS_DebugOutPtr:

	PUSH	R0
	IN		R0,SREG
	CLI

	LDI		Zl, 0xA0		;; Zl:Zh <- USARTD0_base_addr
	LDI		Zh, 0x09

dop_01:
	LDD		R18, Z + 1
	SBRS	R18, 5
	RJMP	dop_01

	STD		Z + 0, p1l		;; Send first data byte

dop_02:
	LDD		R18, Z + 1
	SBRS	R18, 5
	RJMP	dop_02

	STD		Z + 0, p1h		;; Send second data byte

	EOR		p1l,p1h

dop_03:
	LDD		R18, Z + 1
	SBRS	R18, 5
	RJMP	dop_03

	STD		Z + 0, p1l		;; Send checksum = xor ( p1l, p1h )

	OUT		SREG,R0
	POP		R0

ret

	;; TBD

	ret
	
	_ENDFUNC	;; _FUNCTION(RTOS_DebugOutPtr)

/******************************************************************************
** Prototype:	unsigned char RTOS_DebugOutByte( RTOS_U8_t data )
** Description:	
** Passed:		p1l - data byte to log
** Returns:		none
** Uses:		none
** Calls:		none
** Assumes:		none
** Notes:		For Debug purpose only! Don't use in release version!
******************************************************************************/
	_FUNCTION(RTOS_DebugOutByte)

RTOS_DebugOutByte:

	PUSH	R0
	IN		R0,SREG
	CLI

	LDI		Zl, 0xA0		;; Zl:Zh <- USARTD0_base_addr
	LDI		Zh, 0x09

wt_01:
	LDD		R18, Z + 1
	SBRS	R18, 5
	RJMP	wt_01

	STD		Z + 0, p1l		;; Send first data byte

	LDI		p1h, 0x80		;; p1h <- log data identifier, message length = 0 (single data byte);

wt_02:
	LDD		R18, Z + 1
	SBRS	R18, 5
	RJMP	wt_02

	STD		Z + 0, p1h		;; Send second data byte

	EOR		p1l,p1h

wt_03:
	LDD		R18, Z + 1
	SBRS	R18, 5
	RJMP	wt_03

	STD		Z + 0, p1l		;; Send checksum = xor ( p1l, p1h )

	OUT		SREG,R0
	POP		R0

	ret
	
	_ENDFUNC	;; _FUNCTION(RTOS_DebugOutByte)


/******************************************************************************
** Prototype:	void RTOS_DebugOutBuff( void * ptr, RTOS_U8_t size )
** Description:	
** Passed:		p1l:p1h - pointer to data buff to log it out
**				p2l		- number of bytes in buff 
** Returns:		none
** Uses:		none
** Calls:		none
** Assumes:		size = 1...127
** Notes:		For Debug purpose only! Don't use in release version!
******************************************************************************/
	_FUNCTION(RTOS_DebugOutBuff)

RTOS_DebugOutBuff:

	PUSH	R0
	IN		R0,SREG
	CLI						;; Enter critical

	PUSH	Yh				;; preserve Yl:Yh (frame pointer) 
	PUSH	Yl				;; from changes

	LDI		Zl, 0xA0		;; Zl:Zh <- USARTD0_base_addr
	LDI		Zh, 0x09

	MOVW	Yl, p1l			;; Yl:Yh <- p1l:p1h	(pointer to the data buff)
							;; R24, R25 (p1l:p1h) are free to reuse

dob_loop:

	LD		R25, Y+			;; load data byte to R23
							;; R23 <- (Yl:Yh), Yl:Yh += 1

dob_wt_01:
	LDD		R24, Y + 1
	SBRS	R24, 5
	RJMP	dob_wt_01

	STD		Z + 0, R25		;; Send first <<data>> byte

	LDI		R23, 0x80		;; R23 <- 0x80 (indicates data transmission)
	ADD		R23, R22		;; R23 |= size 

dob_wt_02:
	LDD		R24, Z + 1
	SBRS	R24, 5
	RJMP	dob_wt_02

	STD		Z + 0, R23		;; Send second <<id/cnt>> byte

	EOR		R25, R23		;; R25 <- CRC calculate checksum

dob_wt_03:
	LDD		R24, Z + 1
	SBRS	R24, 5
	RJMP	dob_wt_03

	STD		Z + 0, R25		;; Send third <<crc>> byte

	DEC		R22				;; decrement byte counter
	BRNE	dob_loop		;; loop if not zero

	POP		Yl				;; restore frame pointer 
	POP		Yh				;; (Yl:Yh) from stack

	OUT		SREG,R0			;; Leave critical
	POP		R0			

	ret						;; return :)
	
	_ENDFUNC	;; _FUNCTION(RTOS_DebugOutBuff)

/******************************************************************************
** Prototype:	unsigned char RTOS_DebugOutSync( void )
** Description:	send predefined byte sequence ( 0x33, 0x55, 0xAA )
** Passed:		none
** Returns:		none
** Uses:		none
** Calls:		none
** Assumes:		none
** Notes:		For Debug purpose only! Don't use in release version!
******************************************************************************/
	_FUNCTION(RTOS_DebugOutSync)

RTOS_DebugOutSync:

	LDI		Zl, 0xA0		;; Zl:Zh <- USARTD0_base_addr
	LDI		Zh, 0x09

	PUSH	R0
	IN		R0,SREG
	CLI						;; Enter Critical Section

dos_wt_01:
	LDD		R18, Z + 1		
	SBRS	R18, 5
	RJMP	dos_wt_01		;; wait until USART Tx Data Buffer is ready

	LDI		p2l, 0x33
	STD		Z + 0, p2l		;; Send 0x33

dos_wt_02:
	LDD		R18, Z + 1		
	SBRS	R18, 5
	RJMP	dos_wt_02		;; wait until USART Tx Data Buffer is ready

	LDI		p2l, 0xAA
	STD		Z + 0, p2l		;; Send 0xAA

dos_wt_03:
	LDD		R18, Z + 1		
	SBRS	R18, 5
	RJMP	dos_wt_03		;; wait until USART Tx Data Buffer is ready

	LDI		p2l, 0x55
	STD		Z + 0, p2l		;; Send 0x55


	OUT		SREG,R0			;; Leave Critical Section
	POP		R0			

	ret						;; return :)

	_ENDFUNC	;; _FUNCTION(RTOS_DebugOutSync)


/******************************************************************************
** Prototype:	unsigned char RTOS_DebugOutAddr( void * addr )
** Description:	
** Passed:		p1l:p1h - pointer to the constant string in ROM
** Returns:		none
** Uses:		none
** Calls:		none
** Assumes:		none
** Notes:		For Debug purpose only! Don't use in release version!
******************************************************************************/
	_FUNCTION(RTOS_DebugOutAddr)

RTOS_DebugOutAddr:

	LDI		Zl, 0xA0		;; Zl:Zh <- USARTD0_base_addr
	LDI		Zh, 0x09

	MOVW	p2l, p1l		;; Calculate ID/CS byte value
	EOR		p2l, p2h		;; p2l = LOW(addr) XOR HIGH(addr)
	ANDI	p2l, 0x0F		;; p2l &= 0x7F
	ORI		p2l, 0x60		;; p2l |= 0x60	

	PUSH	R0
	IN		R0,SREG
	CLI						;; Enter Critical Section

doa_wt_01:
	LDD		R18, Z + 1		
	SBRS	R18, 5
	RJMP	doa_wt_01		;; wait until USART Tx Data Buffer is ready

	STD		Z + 0, p2l		;; Send ID/CS byte

doa_wt_02:
	LDD		R18, Z + 1
	SBRS	R18, 5
	RJMP	doa_wt_02		;; wait until USART Tx Data Buffer is ready

	STD		Z + 0, p1l		;; Send LOW(addr)

doa_wt_03:
	LDD		R18, Z + 1
	SBRS	R18, 5
	RJMP	doa_wt_03		;; wait until USART Tx Data Buffer is ready

	STD		Z + 0, p1h		;; Send HIGH(byte)

	OUT		SREG,R0			;; Leave Critical Section
	POP		R0			

	ret						;; return :)

	_ENDFUNC	;; _FUNCTION(RTOS_DebugOutAddr)

/******************************************************************************
** Prototype:	void RTOS_DebugOutDump( void )
** Description:	This function dumps the RTOS state
** Passed:		none
** Returns:		none
** Uses:		none
** Calls:		none
** Assumes:		
** Notes:		For Debug purpose only! Don't use in release version!
******************************************************************************/

	_FUNCTION( RTOS_DebugOutDump )

RTOS_DebugOutDump:

	ldi			p1l,lo8(RtosKernelData)
	ldi			p1h,hi8(RtosKernelData)

	ldi			p2l,RtosKernelDataSz-1

	_ENDFUNC	;; _FUNCTION(RTOS_DebugOutDump)

/******************************************************************************
** Prototype:	void RTOS_DebugOutData( void * data, RTOS_U8_t size )
** Description:	
** Passed:		p1l:p1h - pointer to data buff to log it out
**				p2l		- number of bytes in data buff 
** Returns:		none
** Uses:		none
** Calls:		none
** Assumes:		size = 0..31 (max buff length is 32 bytes)
** Notes:		For Debug purpose only! Don't use in release version!
******************************************************************************/
	_FUNCTION(RTOS_DebugOutData)

RTOS_DebugOutData:

	PUSH	Yh				;; preserve Yl:Yh (frame pointer) 
	PUSH	Yl				;; from changes

	LDI		Zl, 0xA0		;; Zl:Zh <- USARTD0_base_addr
	LDI		Zh, 0x09

	MOVW	Yl, p1l			;; Yl:Yh <- p1l:p1h	(pointer to the data buff)
							;; R24, R25 (p1l:p1h) are free to reuse

	ANDI	p2l, 0x0F		;; Calculate ID/NN byte value
	ORI		p2l, 0x90		;; 

	PUSH	R0				;; Enter critical
	IN		R0,SREG
	CLI						

dod_wt_01:
	LDD		R24, Z + 1
	SBRS	R24, 5
	RJMP	dod_wt_01		;; wait until USART Tx Data Buffer is ready

	STD		Z + 0, p2l		;; Send <<ID/NN>> byte

	ANDI	p2l, 0x0F		;; init counter
	INC		p2l
	CLR		p1h				;; reset check sum

dod_loop:

	LD		p2h, Y+			;; load data byte to p2h, increment pointer 
							;; p2h <- (Yl:Yh), Yl:Yh += 1

dod_wt_02:
	LDD		R24, Z + 1
	SBRS	R24, 5
	RJMP	dod_wt_02		;; wait until USART Tx Data Buffer is ready

	STD		Z + 0, p2h		;; Send <<DATA>> byte

	EOR		p1h, p2h		;; update check sum 

	DEC		p2l				;; decrement byte counter
	BRNE	dod_loop		;; loop if not zero

dod_wt_03:
	LDD		R24, Z + 1
	SBRS	R24, 5
	RJMP	dod_wt_03		;; wait until USART Tx Data Buffer is ready

	STD		Z + 0, p1h		;; Send <<CS>> byte

	OUT		SREG,R0			;; Leave critical
	POP		R0			


	POP		Yl				;; restore frame pointer 
	POP		Yh				;; (Yl:Yh) from stack


	ret						;; return :)

	_ENDFUNC	;; _FUNCTION(RTOS_DebugOutData)


	_END
